#include "../include/BTservice/TFtranser.h"
#include "ros/ros.h"
void TFtranser::odomCB(const nav_msgs::OdometryConstPtr &odomPtr)
{
    try
    {
        listener.lookupTransform("/camera_init", "/body", ros::Time(0), radar_pose);
    }
    catch (tf::TransformException &ex)
    {
        ROS_ERROR("%s", ex.what());
        return;
    }
    // ROS_INFO("%f",radar_pose.getOrigin().getY());

    tf::StampedTransform radar_to_base_trans;
    radar_to_base_trans.stamp_ = ros::Time::now();
    radar_to_base_trans.setRotation(radar_pose.getRotation().inverse());
    radar_to_base_trans.frame_id_ = "/body";
    radar_to_base_trans.child_frame_id_ = "/base_link";
    radar_to_base_trans.setOrigin(radar_to_base_tvec_xyz);

    broadcaster.sendTransform(radar_to_base_trans);
}

bool TFtranser::transSer(robots_msgs::TransRequest &req, robots_msgs::TransResponse &resp)
{
    std::string base_frame, target_frame;
    geometry_msgs::PointStamped in, out;

    base_frame = "/gimbal";
    target_frame = "/base_link";

    // 将云台检测到的目标位置从云台系转换到底盘系
    in.header.stamp = ros::Time();
    in.header.frame_id = base_frame;
    in.point.x = req.detect_msg.target_x;
    in.point.y = req.detect_msg.target_y;
    in.point.z = req.detect_msg.target_z;

    listener.transformPoint(target_frame, in, out);

    if (isnan(out.point.x) || isnan(out.point.y) || isnan(out.point.z))
    {
        return false;
    }

    resp.body_x = out.point.x;
    resp.body_y = out.point.y;
    resp.body_z = out.point.z;
    resp.tiny_gimbal_aim_pitch = 0;
    resp.tiny_gimbal_aim_yaw = 0;
    return true;
}

TFtranser::TFtranser()
{
    ros::NodeHandle nh;
    odom_sub = nh.subscribe<nav_msgs::Odometry>("/aft_mapped_to_init", 1, &TFtranser::odomCB, this);

    ros::NodeHandle nh2;
    transServer = nh.advertiseService("/TransSer", &TFtranser::transSer, this);
}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "TFtrans");

    TFtranser tft;
    ros::spin();
    return 0;
}
